#include "BundlerParser.h"
#include "Util.h"
#include <cstdarg>

CBundlerParser::CBundlerParser() : m_BundleFileName(NULL)
{
}

CBundlerParser::CBundlerParser(CCameraMesh* pMesh, ImagesVector* pImages) : m_BundleFileName(NULL) , m_pMesh(pMesh), m_pImages(pImages)
{
}

void CBundlerParser::ParseBundle(char* BundleFileName, vector<CMeshInstance*>* pModels)
{
	FILE* BundleFile = fopen(BundleFileName, "r");
	VISIO_ASSERT(BundleFile);

	char buff[100];
	UINT NumberOfCameras = 0; 
	UINT NumberOfPoints = 0;
	// first line the file is just an head need to skip that
	fgets(buff, 100, BundleFile);

	// getting the number of cameras and number of points 
	fscanf(BundleFile, "%d %d", &NumberOfCameras, &NumberOfPoints);

	for (UINT i = 0; i < min(NumberOfCameras, m_pImages->size()); i++)
	{
		SParsedCamera CurrCamera;
		ParseCamera(BundleFile, CurrCamera);
		CCamera* newCamera = ConvertParsedCamera(&CurrCamera);
		pModels->push_back(newCamera->CreateMeshInstance());
	}
	
	double* points = new double[NumberOfPoints*3];
	float* colors = new float[NumberOfPoints * 3];
	for (UINT i = 0; i < NumberOfPoints; i++)
	{
		double x[3];
		int rgb[3];
		
		int tempI;
		double tempD;
		int NumberOfViews = 0;
		fscanf(BundleFile, "%lf %lf %lf", x, x+1, x+2);
		//read color currently ignore it
		fscanf(BundleFile, "%d %d %d", rgb, rgb + 1, rgb + 2);
		fscanf(BundleFile, "%d", &NumberOfViews);
		for (UINT j = 0; j < NumberOfViews; j++)
		{
			fscanf(BundleFile, "%d %d %lf %lf", &tempI, &tempI, &tempD, &tempD);
		}
		float col[] = {UNORM_TO_FLOAT(rgb[0]), UNORM_TO_FLOAT(rgb[1]), UNORM_TO_FLOAT(rgb[2])};
		memcpy(points + 3*i, x, sizeof(double)*3);
		memcpy(colors + 3*i, col, sizeof(float) * 3);


	}

	CPointCloudMesh* pMesh = new CPointCloudMesh(points, colors, NumberOfPoints);
	pModels->push_back(pMesh->CreateInstance());
	delete[] points;
	delete[] colors;
}

CCamera* CBundlerParser::ConvertParsedCamera(SParsedCamera* pPCam)
{
	VISIO_ASSERT(m_pMesh);
	VISIO_ASSERT(m_pImages);
	cv::Mat R = cv::Mat(3, 3, CV_64F, pPCam->R);
	cv::Mat t = cv::Mat(3, 1, CV_64F, pPCam->t);
	
	CCamera* newCam = new CCamera(m_pMesh, R, t, (*m_pImages)[pPCam->index], pPCam->f);
	return newCam;
}

void CBundlerParser::ParseCamera(FILE* pBundleFile, SParsedCamera& newParsedCamera)
{
	// camera format is 
	// image index
	// intrin parameters f, k1, k2
	// 3x3 rotation matrix R
	// 3x1 translation vector t

	fscanf(pBundleFile, "%d", &newParsedCamera.index);

	GetDoubleFromFile(pBundleFile, 3, &newParsedCamera.f, &newParsedCamera.k1, &newParsedCamera.k2);
	
	GetDoubleFromFile(pBundleFile, 9, newParsedCamera.R);

	GetDoubleFromFile(pBundleFile, 3, newParsedCamera.t);
}

double CBundlerParser::GetSingleDoubleFromFile(FILE* pFile) const
{
	char buff[50];
	fscanf(pFile, "%s", buff);
	double tDouble = atof(buff);
	return tDouble;
}

void CBundlerParser::GetDoubleFromFile(FILE* pFile, UINT NumberOfFloatsToGet, ...) const
{
	va_list ap;
	va_start(ap, NumberOfFloatsToGet);
		for (UINT i = 0; i < NumberOfFloatsToGet; i++)
		{
			double* pDoubleArg = va_arg(ap, double*);
			*pDoubleArg = GetSingleDoubleFromFile(pFile);
		}
	va_end(ap);
}

void CBundlerParser::GetDoubleFromFile(FILE* pFile, UINT NumberOfFloatsToGet, double* pDouble) const
{
	for (UINT i = 0; i < NumberOfFloatsToGet; i++)
	{
		double newDouble = GetSingleDoubleFromFile(pFile);
		pDouble[i] = newDouble;
	}
}